#include"manifoldCommunication.h"
#include <string>

using namespace robowalker;
/*@brief  设置串口通信速率
 *@param  fd     类型 int  打开串口的文件句柄
 *@param  speed  类型 int  串口速度
 *@return  void
 */
int speed_arr[] = {
        B230400,
        B115200,
        B57600,
        B38400,
        B19200,
        B9600,
        B4800,
        B2400,
        B1200,
        B300,
        B38400,
        B19200,
        B9600,
        B4800,
        B2400,
        B1200,
        B300,
};

int name_arr[] = {
        230400,
        115200,
        57600,
        38400,
        19200,
        9600,
        4800,
        2400,
        1200,
        300,
        38400,
        19200,
        9600,
        4800,
        2400,
        1200,
        300,
};


void set_speed(int fd, int speed)
{
    int i;
    int status;
    struct termios Opt;
    tcgetattr(fd, &Opt);
    for (i = 0; i < sizeof(speed_arr) / sizeof(int); i++)
    {
        if (speed == name_arr[i])
        {
            tcflush(fd, TCIOFLUSH);
            cfsetispeed(&Opt, speed_arr[i]);
            cfsetospeed(&Opt, speed_arr[i]);
            status = tcsetattr(fd, TCSANOW, &Opt);
            if (status != 0)
                perror("tcsetattr fd1");
            return;
        }
        tcflush(fd, TCIOFLUSH);
    }
}

/*
 *@brief  设置串口数据位，停止位和效验位
 *@param  fd       类型  int  打开的串口文件句柄*
 *@param  databits 类型  int  数据位   取值 为 7 或者8*
 *@param  stopbits 类型  int  停止位   取值为 1 或者2*
 *@param  parity   类型  int  效验类型 取值为N,E,O,,S
 */
int set_Parity(int fd, int databits, int stopbits, int parity)
{
    struct termios options;
    if (tcgetattr(fd, &options) != 0)
    {
        perror("SetupSerial 1");
        return (FALSE);
    }
    options.c_cflag &= ~CSIZE;
    switch (databits) /*设置数据位数*/
    {
    case 7:
        options.c_cflag |= CS7;
        break;
    case 8:
        options.c_cflag |= CS8;
        break;
    default:
        fprintf(stderr, "Unsupported data size\n");
        return (FALSE);
    }
    switch (parity)
    {
    case 'n':
    case 'N':
        options.c_cflag &= ~PARENB; /* Clear parity enable */
        options.c_iflag &= ~INPCK;  /* Enable parity checking */
        break;
    case 'o':
    case 'O':
        options.c_cflag |= (PARODD | PARENB); /* 设置为奇效验*/
        options.c_iflag |= INPCK;             /* Disnable parity checking */
        break;
    case 'e':
    case 'E':
        options.c_cflag |= PARENB;  /* Enable parity */
        options.c_cflag &= ~PARODD; /* 转换为偶效验*/
        options.c_iflag |= INPCK;   /* Disnable parity checking */
        break;
    case 'S':
    case 's': /*as no parity*/
        options.c_cflag &= ~PARENB;
        options.c_cflag &= ~CSTOPB;
        break;
    default:
        fprintf(stderr, "Unsupported parity\n");
        return (FALSE);
    }
    /* 设置停止位*/
    switch (stopbits)
    {
    case 1:
        options.c_cflag &= ~CSTOPB;
        break;
    case 2:
        options.c_cflag |= CSTOPB;
        break;
    default:
        fprintf(stderr, "Unsupported stop bits\n");
        return (FALSE);
    }
    /* Set input parity option */
    if (parity != 'n')
        options.c_iflag |= INPCK;
    options.c_cc[VTIME] = 150; // 15 seconds
    options.c_cc[VMIN] = 0;

    tcflush(fd, TCIFLUSH); /* Update the options and do it NOW */
    if (tcsetattr(fd, TCSANOW, &options) != 0)
    {
        perror("SetupSerial 3");
        return (FALSE);
    }

    /*Set these below to solve 0x0A --> 0x0D 0x0A problem */
    options.c_iflag &= ~(INLCR | ICRNL | IGNCR);
    options.c_oflag &= ~(ONLCR | OCRNL | ONOCR | ONLRET); 
    tcsetattr(fd,TCSANOW,&options);

    return (TRUE);
}


/*
 *@brief 打开串口
 */
int openDev(const char *Dev)
{
    int fd = open(Dev, O_RDWR | O_NONBLOCK); //| O_NOCTTY | O_NDELAY
    if (-1 == fd)
    { /*设置数据位数*/
        perror("Can't Open Serial Port");
        return -1;
    }
    else
        return fd;
}

/*
* @brief 解析收到的字符串，转换为数字 
*/
void CommunicationManager::stringToCarInfo(car_info* info_ptr, char* recv_buff) const
{
    // auto s_vx = string(recv_buff+6, 6);
    // printf("string vx: %s\n", s_vx.c_str());

    info_ptr->vy = atoi(string(recv_buff, 6).c_str());
    info_ptr->vx = atoi(string(recv_buff+6, 6).c_str());
    info_ptr->pitch = atoi(string(recv_buff+12, 6).c_str());
    info_ptr->yaw = atoi(string(recv_buff+18, 6).c_str());

    info_ptr->color = (recv_buff[24] == '1') ? true : false;
    info_ptr->is_auto_aiming_open = (recv_buff[25]== '1') ? true : false;
    info_ptr->is_shooting_energy = (recv_buff[26] == '1') ? true : false;
}


vision_info assembleVisionInfoStruct(double angleX, double angleY, double dist)
{
    return {int16_t(angleX * 100), int16_t(angleY * 100), int16_t(dist * 1000)};
}

vision_info assembleVisionInfoStruct(double angleX, double angleY, double predict_angleX, double predict_angleY, double dist,
double predict_vx, double predict_vy)
{
    return {int16_t(angleX * 100),
            int16_t(angleY * 100),
            int16_t(predict_angleX * 100),
            int16_t(predict_angleY * 100),
            int16_t(dist),
            int16_t(predict_vx *1000),
            int16_t(predict_vy *1000)};
}

CommunicationManager::CommunicationManager(const string &serialPortPath, const int baudrate) {
    const char *dev = serialPortPath.c_str();
    is_serial_open_success = false;
    fd = openDev(dev); //1open uart

    struct termios options;

    if (fd <= 0)
    {
        printf("[Error] Can't Open Serial Port!\n");
        is_serial_open_success = false;
    }

    if (tcgetattr(fd, &options) != 0)
    {
        perror("GetupSerial 1");
    }
    cfmakeraw(&options);
    // options.c_cflag |= (tcflag_t) (CLOCAL | CREAD);

    // options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG | IEXTEN);
    // options.c_oflag &= (tcflag_t) ~(OPOST);
    // options.c_iflag &= (tcflag_t) ~(INLCR | IGNCR | ICRNL | IGNBRK | IUCLC | IXON | IXOFF | IXANY);
    // options.c_cflag &= (tcflag_t) ~(CRTSCTS);
    tcflush(fd, TCIFLUSH); /* Update the options and do it NOW */
    if (tcsetattr(fd, TCSANOW, &options) != 0)
    {
        printf("[Error]Set Serial Port Error\n");
        is_serial_open_success = false;
    }
    tcflush(fd, TCIFLUSH); /* Update the options and do it NOW */

    set_speed(fd, baudrate); //2set speed
   
    if (set_Parity(fd, 8, 1, 'N') == FALSE) //3set parity
    {
        printf("[Error] Set Parity Error\n");
        is_serial_open_success = false;
    }

    is_serial_open_success = true;
}

bool CommunicationManager::is_serial_open() const {
    return is_serial_open_success;
}

int CommunicationManager::manifoldReceiveOnce(car_info* receive_data) const {
    const int recv_buff_len = sizeof(car_info)+2;
    char recv_buff[recv_buff_len];
    int nread = read(fd, recv_buff,recv_buff_len );
    if(nread > 0)
    {
        //printf("\n received data! Len %d\n", nread);
        // stringToCarInfo(receive_data, recv_buff);
        if(*(uint16_t*)recv_buff==0xCDAB)
        {
            memcpy(receive_data, recv_buff+2, sizeof(car_info));
            // printf("pitch: %f, yaw: %f, vx: %f, vy: %f. \n \
            //    color: %d, is_auto_aiming_open: %d, is_shooting_energy: %d\n\n",
            //    float(receive_data->pitch) ,
            //    float(receive_data->yaw) ,
            //    float(receive_data->vx),
            //    float(receive_data->vy) ,
            //    receive_data->color,
            //    receive_data->is_auto_aiming_open,
            //    receive_data->is_shooting_energy
            // );
        }
    }
    return nread;
}

int CommunicationManager::manifoldSendOnce(const vision_info send_data) const {
    char send_buff[maxBufferSize];
    send_buff[0]=0xAB;
    send_buff[1]=0xCD;
    if(sizeof(send_data) > maxBufferSize)
    {
        printf("Data to be sent exceeds max buffer size!\n");
        return FAIL;
    }
    memcpy(send_buff+2,&send_data,sizeof(send_data));
    int ret = write(fd, send_buff, sizeof(send_data)+2);
    // printf("[Communication] manifold send once. ");
    return ret;
}

CommunicationManager::~CommunicationManager() {
    close(fd);
}